﻿#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <vector>
#include <cmath>
#include "circlefitsolver.h"
#include <QDebug>
using namespace std;
using namespace cv;

bool fitCircle(const std::vector<cv::Point2i> &points, double &center_x, double &center_y, double &radius)
{
     center_x = 0.0f;
     center_y = 0.0f;
     radius = 0.0f;
     if (points.size() < 3)
     {
         return false;
     }

     double sum_x = 0.0f, sum_y = 0.0f;
     double sum_x2 = 0.0f, sum_y2 = 0.0f;
     double sum_x3 = 0.0f, sum_y3 = 0.0f;
     double sum_xy = 0.0f, sum_x1y2 = 0.0f, sum_x2y1 = 0.0f;

     int N = points.size();
     for (int i = 0; i < N; i++)
     {
         double x = points[i].x;
         double y = points[i].y;
         double x2 = x * x;
         double y2 = y * y;
         sum_x += x;
         sum_y += y;
         sum_x2 += x2;
         sum_y2 += y2;
         sum_x3 += x2 * x;
         sum_y3 += y2 * y;
         sum_xy += x * y;
         sum_x1y2 += x * y2;
         sum_x2y1 += x2 * y;
     }

     double C, D, E, G, H;
     double a, b, c;

     C = N * sum_x2 - sum_x * sum_x;
     D = N * sum_xy - sum_x * sum_y;
     E = N * sum_x3 + N * sum_x1y2 - (sum_x2 + sum_y2) * sum_x;
     G = N * sum_y2 - sum_y * sum_y;
     H = N * sum_x2y1 + N * sum_y3 - (sum_x2 + sum_y2) * sum_y;
     a = (H * D - E * G) / (C * G - D * D);
     b = (H * C - E * D) / (D * D - G * C);
     c = -(a * sum_x + b * sum_y + sum_x2 + sum_y2) / N;

     center_x = a / (-2);
     center_y = b / (-2);
     radius = sqrt(a * a + b * b - 4 * c) / 2;
     return true;
}


bool fitCircle(const cv::Mat &image, double &center_x, double &center_y, double &radius, unsigned char threshold)
{
    if( image.empty() )
    {
        center_x = 0;
        center_y = 0;
        radius = 0;
        return false;
    }

    cv::Mat grayImage;

    if(image.type() == CV_8UC4) // 暂时只支持 BGR 和 GRAY 两种颜色模式
    {
        cv::cvtColor( image, grayImage, CV_BGRA2GRAY );
    }
    else if(image.type() == CV_8UC3)
    {
        cv::cvtColor( image, grayImage, CV_BGR2GRAY );
    }
    else
    {
        grayImage = image.clone();
    }

    if( threshold == 0) // 正常的阈值分割不会用到 0 作为阈值
    {
        cv::threshold(grayImage, grayImage, 128, 255, CV_THRESH_OTSU);
    }
    else
    {
        cv::threshold(grayImage, grayImage, threshold, 255, CV_THRESH_BINARY);
    }
    //cv::threshold(grayImage, image, 128, 255, CV_THRESH_OTSU);
    //cv::imshow("image", image);
    cv::Mat kernel = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(7, 7));
    cv::morphologyEx(image, image, cv::MORPH_OPEN, kernel);
    cv::morphologyEx(image, image, cv::MORPH_CLOSE, kernel);
    cv::Canny(image, image, 128, 200, 3);

    std::vector<cv::Point2i> points;

    for(int row = 0; row < image.rows; row++)
    {
        const uchar * pLine = image.ptr<const uchar>(row);
        for(int col = 0; col < image.cols; col++)
        {
            if(pLine[col] > 128)
            {
                points.push_back(cv::Point2i(col, row));
            }
        }
    }
    //radius = -1;
    //return true;
    return fitCircle(points, center_x, center_y, radius);
}


bool fitInnerCircle0(const cv::Mat &image, double &center_x, double &center_y, double &radius, unsigned char threshold)
{
    if( image.empty() )
    {
        center_x = 0;
        center_y = 0;
        radius = 0;
        return false;
    }
    cv::Mat grayImage;

    if(image.type() == CV_8UC4) // 暂时只支持 BGR 和 GRAY 两种颜色模式
    {
        cv::cvtColor( image, grayImage, CV_BGRA2GRAY );
    }
    else if(image.type() == CV_8UC3)
    {
        cv::cvtColor( image, grayImage, CV_BGR2GRAY );
    }
    else
    {
        grayImage = image.clone();
    }
    //imshow("1", grayImage);
    //
    qDebug() << "threshold = " << threshold;
    if( threshold == 0) // 正常的阈值分割不会用到 0 作为阈值
    {
        cv::threshold(grayImage, grayImage, 128, 255, CV_THRESH_OTSU);
    }
    else
    {
        cv::threshold(grayImage, grayImage, threshold, 255, CV_THRESH_BINARY);
    }
    //cv::imshow("image", image);
    //imshow("2", grayImage);
    cv::Mat kernel = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(7, 7));
    cv::morphologyEx(grayImage, grayImage, cv::MORPH_OPEN, kernel);
    cv::morphologyEx(grayImage, grayImage, cv::MORPH_CLOSE, kernel);
    cv::Canny(grayImage, grayImage, 128, 200, 3);


    std::vector<cv::Point2i> points;

    for(int row = 0; row < grayImage.rows; row++)
    {
        const uchar * pLine = grayImage.ptr<const uchar>(row);
        for(int col = 0; col < grayImage.cols; col++)
        {
            if(pLine[col] > 128)
            {
                points.push_back(cv::Point2i(col, row));
            }
        }
    }
    CircleFitSolver solver;
    radius = -1;
    //bool ret = solver.circleFitL1(points, center_x, center_y, radius);
    //cv::imshow("image", image);
    //cv::imshow("i", grayImage);
    bool ret = fitCircle(points, center_x, center_y, radius);
    // 对于圆环来说，这里拟合出的圆介于内外圆之间
    if( !ret )
    {
        return false;
    }
    //cv::Mat mask(image.rows, image.cols, CV_8U, cv::Scalar(0, 0, 0));
    //cv::circle(mask, cv::Point(center_x, center_y), radius, cv::Scalar(255), -1);
    //cv::bitwise_and(grayImage, mask, grayImage);


    // 抹去外圆
    std::vector<cv::Point2i> p2;
    int N = points.size();
    for(int i = 0; i < N; i++)
    {
        int col = points[i].x;
        int row = points[i].y;
        if(hypot(col - center_x, row - center_y) < radius)
        {
            p2.push_back(cv::Point2i(col, row));
        }
    }
    // 再次拟合就只剩下内圆了
    //return true;
    //radius = -1;
    return solver.circleFitL1(p2, center_x, center_y, radius);
    //cv::circle(grayImage, cv::Point(center_x, center_y), radius, cv::Scalar(255), 1);
    //cv::namedWindow("debug", cv::WINDOW_NORMAL);imshow("debug", grayImage);
    //return true;
    //return fitCircle(p2, center_x, center_y, radius);
}



bool fitInnerCircle(const cv::Mat &image, double &center_x, double &center_y, double &radius, unsigned char threshold)
{
    if( image.empty() )
    {
        center_x = 0;
        center_y = 0;
        radius = 0;
        return false;
    }
    cv::Mat grayImage;

    if(image.type() == CV_8UC4) // 暂时只支持 BGR 和 GRAY 两种颜色模式
    {
        cv::cvtColor( image, grayImage, CV_BGRA2GRAY );
    }
    else if(image.type() == CV_8UC3)
    {
        cv::cvtColor( image, grayImage, CV_BGR2GRAY );
    }
    else
    {
        grayImage = image.clone();
    }
    //imshow("1", grayImage);
    //
    qDebug() << "threshold = " << threshold;
    if( threshold == 0) // 正常的阈值分割不会用到 0 作为阈值
    {
        //cv::threshold(grayImage, grayImage, 128, 255, CV_THRESH_OTSU);
    }
    else
    {
        //cv::threshold(grayImage, grayImage, threshold, 255, CV_THRESH_BINARY);
    }
    //cv::imshow("image", image);
    //cv::imshow("2", grayImage);
    cv::Mat kernel = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(7, 7));
    cv::morphologyEx(grayImage, grayImage, cv::MORPH_OPEN, kernel);
    cv::morphologyEx(grayImage, grayImage, cv::MORPH_CLOSE, kernel);
    cv::Canny(grayImage, grayImage, 100, 150, 3);
    //cv::imshow("3", grayImage);

    std::vector<cv::Point2i> points;
    std::vector<cv::Point2i> p[5];

    double center_x_tmp[6];
    double center_y_tmp[6];
    double radius_tmp[6];

    CircleFitSolver solver;
    radius = -1;

    for(int row = 0; row < grayImage.rows; row++)
    {
        const uchar * pLine = grayImage.ptr<const uchar>(row);
        for(int col = 0; col < grayImage.cols; col++)
        {
            if(pLine[col] > 128)
            {
                points.push_back(cv::Point2i(col, row));
            }
        }
    }
    int N = points.size();

    if( !solver.circleFitL1(points, center_x, center_y, radius) )
    {
        return false;
    }
    center_x_tmp[0]=center_x;
    center_y_tmp[0]=center_y;
    radius_tmp[0]=radius;

    for(int i=0;i<5;i++)
    {
        for(int j = 0; j < N; j++)
        {
            int col = points[j].x;
            int row = points[j].y;
            if(hypot(col - center_x_tmp[i], row - center_y_tmp[i]) < radius_tmp[i]+i)
            {
                p[i].push_back(cv::Point2i(col, row));
            }
        }
        bool ret = solver.circleFitL1(p[i], center_x, center_y, radius);
        if( !ret )return false;
        center_x_tmp[i+1]=center_x;
        center_y_tmp[i+1]=center_y;
        radius_tmp[i+1]=radius;
    }

    qDebug() << "points.size()=" << points.size() << "center_x_tmp[0]= " << center_x_tmp[0]<< "center_y_tmp[0]= " << center_y_tmp[0]<< "radius_tmp[0]= " << radius_tmp[0];
    qDebug() << "p[0].size()=" << p[0].size() << "center_x_tmp[1]= " << center_x_tmp[1] << "center_y_tmp[1]= " << center_y_tmp[1] << "radius_tmp[1]= " << radius_tmp[1];
    qDebug() << "p[1].size()=" << p[1].size() << "center_x_tmp[2]= " << center_x_tmp[2] << "center_y_tmp[2]= " << center_y_tmp[2] << "radius_tmp[2]= " << radius_tmp[2];
    qDebug() << "p[2].size()=" << p[2].size() << "center_x_tmp[3]= " << center_x_tmp[3] << "center_y_tmp[3]= " << center_y_tmp[3] << "radius_tmp[3]= " << radius_tmp[3];
    qDebug() << "p[3].size()=" << p[3].size() << "center_x_tmp[4]= " << center_x_tmp[4] << "center_y_tmp[4]= " << center_y_tmp[4] << "radius_tmp[4]= " << radius_tmp[4];
    qDebug() << "p[4].size()=" << p[4].size() << "center_x_tmp[5]= " << center_x_tmp[5] << "center_y_tmp[5]= " << center_y_tmp[5] << "radius_tmp[5]= " << radius_tmp[5];

    int m_error=0;

    if( radius_tmp[1]>radius_tmp[0])
    {
        m_error=-1;//半径发散
        qDebug() << "半径发散" ;
    }
    if( radius_tmp[2]>radius_tmp[1])
    {
        m_error=-2;//半径发散
        qDebug() << "半径发散" ;
    }

    double move_4_5=hypot(center_x_tmp[5] - center_x_tmp[4], center_y_tmp[5] - center_y_tmp[4]);
    double move_3_4=hypot(center_x_tmp[4] - center_x_tmp[3], center_y_tmp[4] - center_y_tmp[3]);
    double move_2_3=hypot(center_x_tmp[3] - center_x_tmp[2], center_y_tmp[3] - center_y_tmp[2]);
    if( move_3_4 > move_2_3+0.1)
    {
        m_error=-3;//圆心发散
        qDebug() << "圆心发散" ;
    }
    if( move_3_4 > 5.0)
    {
        m_error=-4;//圆心发散
        qDebug() << "圆心发散" ;
    }
    qDebug() << "move_4_5 = " << move_4_5 ;
    qDebug() << "move_3_4 = " << move_3_4 <<"if( move_3_4 > 5.0)";
    qDebug() << "move_2_3 = " << move_2_3 <<" if( move_3_4 > move_2_3+0.1)";

    double s=0;
    N=p[4].size();
    for(int i=0;i<N;i++)
    {
        int col = p[4][i].x;
        int row = p[4][i].y;
        s+=abs(hypot(col - center_x, row - center_y)-radius);
    }
    s=s/p[4].size();
    qDebug() << "s = " << s <<"if( s>2.0 )";
    if( s>2.0 )
    {
        m_error=-5;//拟合度低
        qDebug() << "拟合度低" ;
    }

    double move_0_1=hypot(center_x_tmp[1] - center_x_tmp[0], center_y_tmp[1] - center_y_tmp[0] );
    qDebug() << "move_0_1 = " << move_0_1 <<"if( move_0_1 < 1.0)";
    if( move_0_1 < 1.0)
    {
        m_error=-6;//圆心不动，单圆非环
        qDebug() << "圆心不动，单圆非环" ;
    }

    if( center_x<radius || center_y<radius || center_x+radius>grayImage.cols || center_y+radius>grayImage.rows )
    {
        m_error=-7;//圆超出边界
        qDebug() << "圆超出边界" ;
    }

    double rate_pie=(double)p[4].size()/radius;
    qDebug() << "rate_pie = " << rate_pie <<"if(rate_pie<3.14)";
    if(rate_pie<3.14)
    {
        m_error=-8;//最终留下的点太少  针对 外圆内圆各一半情况
        qDebug() << "最终留下的点太少  针对 外圆内圆各一半情况" ;
    }

    double rate_radius=radius_tmp[0]/radius_tmp[5];
    if(rate_radius>3)
    {
        m_error=-9;//第一次拟合和最后一次拟合半径比太大，说明最终拟合的圆过小
        qDebug() << "第一次拟合和最后一次拟合半径比太大，说明最终拟合的圆过小" ;
    }
    qDebug() << "rate_radius = " << rate_radius <<"if(rate_radius>3)";

    cv::Mat image_error;
    cv::cvtColor(grayImage,image_error,CV_GRAY2BGR);
    cv::circle(image_error, cv::Point(center_x, center_y), radius, cv::Scalar(255,0,0));
    cv::imshow("image_error",image_error);

    cv::circle(grayImage, cv::Point(center_x_tmp[0], center_y_tmp[0]), radius_tmp[0], cv::Scalar(50));
    cv::circle(grayImage, cv::Point(center_x_tmp[1], center_y_tmp[1]), radius_tmp[1], cv::Scalar(100));
    cv::circle(grayImage, cv::Point(center_x_tmp[2], center_y_tmp[2]), radius_tmp[2], cv::Scalar(150));
    cv::circle(grayImage, cv::Point(center_x_tmp[3], center_y_tmp[3]), radius_tmp[3], cv::Scalar(200));
    cv::circle(grayImage, cv::Point(center_x_tmp[4], center_y_tmp[4]), radius_tmp[4], cv::Scalar(250));

    cv::Mat imageshow;
    imageshow=image.clone();
    cv::circle(imageshow, cv::Point(center_x, center_y), radius, cv::Scalar(255,255,255));
    cv::imshow("imageshow",imageshow);
    cv::imshow("grayImage", grayImage);

    return true;

}
